#define BLINKER_BLE
#include <Blinker.h>
#define JOY_1 "JOYKey"

BlinkerButton Button1("BUTTON_1");
BlinkerButton Button2("BUTTON_2");
BlinkerButton Button3("BUTTON_3");
BlinkerButton Button4("BUTTON_4");
BlinkerButton Button5("BUTTON_5");
BlinkerButton Button6("BUTTON_6");
BlinkerText text1("text1");

BlinkerJoystick JOY1(JOY_1);
//定义蓝牙，按钮，摇杆

int FRONT_LEFT_EN = 2;
int FRONT_LEFT_IN1 = 4;
int FRONT_LEFT_IN2 = 16;
int FRONT_RIGHT_EN = 18;
int FRONT_RIGHT_IN3 = 17;
int FRONT_RIGHT_IN4 = 5;
int REAR_LEFT_EN = 12;
int REAR_LEFT_IN1 = 14;
int REAR_LEFT_IN2 = 27;
int REAR_RIGHT_EN = 33;
int REAR_RIGHT_IN3 = 26;
int REAR_RIGHT_IN4 = 25;




int speed;
void Button1_callback(const String & state)
{
   if(state == "tap")
   {
     speed = 64;
   }
}
void Button2_callback(const String & state)
{
    if(state == "tap")
   {
    speed = 128;
   } 
}
void Button3_callback(const String & state)
{
    if(state == "tap")
   {
    speed = 255;
   }
}
//BUTTON123控制电机速度



void Button4_callback(const String & state)
{
   if(state == "tap")
   {
       // 前左电机
  digitalWrite(FRONT_LEFT_IN1, LOW);
  digitalWrite(FRONT_LEFT_IN2, HIGH);
  analogWrite(FRONT_LEFT_EN, 255);

  // 前右电机
  digitalWrite(FRONT_RIGHT_IN3, HIGH);
  digitalWrite(FRONT_RIGHT_IN4, LOW);
  analogWrite(FRONT_RIGHT_EN, 255);

  // 后左电机
  digitalWrite(REAR_LEFT_IN1, LOW);
  digitalWrite(REAR_LEFT_IN2, HIGH);
  analogWrite(REAR_LEFT_EN, 255);

  // 后右电机
  digitalWrite(REAR_RIGHT_IN3, HIGH);
  digitalWrite(REAR_RIGHT_IN4, LOW);
  analogWrite(REAR_RIGHT_EN, 255);
   }
}
void Button5_callback(const String & state)
{
   if(state == "tap")
   {
       // 前左电机
  digitalWrite(FRONT_LEFT_IN1, LOW);
  digitalWrite(FRONT_LEFT_IN2, HIGH);
  analogWrite(FRONT_LEFT_EN, 64);

  // 前右电机
  digitalWrite(FRONT_RIGHT_IN3, HIGH);
  digitalWrite(FRONT_RIGHT_IN4, LOW);
  analogWrite(FRONT_RIGHT_EN, 64);

  // 后左电机
  digitalWrite(REAR_LEFT_IN1, LOW);
  digitalWrite(REAR_LEFT_IN2, HIGH);
  analogWrite(REAR_LEFT_EN, 64);

  // 后右电机
  digitalWrite(REAR_RIGHT_IN3, HIGH);
  digitalWrite(REAR_RIGHT_IN4, LOW);
  analogWrite(REAR_RIGHT_EN, 64);
   }
}
void Button6_callback(const String & state)
{
   if(state == "tap")
   {
       // 前左电机
  digitalWrite(FRONT_LEFT_IN1, HIGH);
  digitalWrite(FRONT_LEFT_IN2, LOW);
  analogWrite(FRONT_LEFT_EN, 64);

  // 前右电机
  digitalWrite(FRONT_RIGHT_IN3, LOW);
  digitalWrite(FRONT_RIGHT_IN4, HIGH);
  analogWrite(FRONT_RIGHT_EN, 64);

  // 后左电机
  digitalWrite(REAR_LEFT_IN1, HIGH);
  digitalWrite(REAR_LEFT_IN2, LOW);
  analogWrite(REAR_LEFT_EN, 64);

  // 后右电机
  digitalWrite(REAR_RIGHT_IN3, LOW);
  digitalWrite(REAR_RIGHT_IN4, HIGH);
  analogWrite(REAR_RIGHT_EN, 64);
   }
}
//BUTTON456控制旋转



void joystick1_callback(uint8_t joystickX, uint8_t joystickY)
{
  // 检查摇杆是否处于中心位置
  float x = (float)(joystickX-128);
  float y = (float)(joystickY-128);
  text1.print(x/y);
  if (joystickX == 128 && joystickY == 128)
  {
    stopMotors();
  }
  else
  {
    // 根据摇杆值判断方向
    if ( y<0 && (y/x<=-2.0f/3.0f || y/x >=2.0f/3.0f) ) // 前
    {
      FRONT();
    }
    else if ( y>0 && (y/x<=-2.0f/3.0f || y/x >=2.0f/3.0f)) // 后
    {
      BEHIND();
    }
    else if (x<0 && (y/x>=-1.0f/3.0f && y/x<=1.0f/3.0f)) //左
    {
      LEFT();
    }
    else if (x>0 && (y/x>=-1.0f/3.0f && y/x<=1.0f/3.0f)) // 右
    {
      RIGHT();
    }
    else if (x<0 && y<0 && (y/x>1.0f/3.0f && y/x<2.0f/3.0f))//左前
    {
      LEFTFRONT();
    }
    else if (x>0 && y>0 && (y/x>1.0f/3.0f && y/x<2.0f/3.0f))//右后
    {
      RIGHTBEHIND();
    }
    else if (x>0 && y<0 && (y/x>-2.0f/3.0f && y/x<-1.0f/3.0f))//右前
    {
      RIGHTFRONT();
    }
    else if (x<0 && y>0 && (y/x>-2.0f/3.0f && y/x<-1.0f/3.0f))//左后
    {
      LEFTBEHIND();
    }
  }
}
//代码中的主要函数，摇杆回调函数，主要是通过遥感的坐标值Y/X的比值控制方向调用下面的方向函数
void FRONT()
{
  // 前左电机
  digitalWrite(FRONT_LEFT_IN1, HIGH);
  digitalWrite(FRONT_LEFT_IN2, LOW);
  analogWrite(FRONT_LEFT_EN, speed);

  // 前右电机
  digitalWrite(FRONT_RIGHT_IN3, LOW);
  digitalWrite(FRONT_RIGHT_IN4, HIGH);
  analogWrite(FRONT_RIGHT_EN, speed);

  // 后左电机
  digitalWrite(REAR_LEFT_IN1, HIGH);
  digitalWrite(REAR_LEFT_IN2, LOW);
  analogWrite(REAR_LEFT_EN, speed);

  // 后右电机
  digitalWrite(REAR_RIGHT_IN3, LOW);
  digitalWrite(REAR_RIGHT_IN4, HIGH);
  analogWrite(REAR_RIGHT_EN, speed);
}
void BEHIND()
{
  // 前左电机
  digitalWrite(FRONT_LEFT_IN1, LOW);
  digitalWrite(FRONT_LEFT_IN2, HIGH);
  analogWrite(FRONT_LEFT_EN, speed);

  // 前右电机
  digitalWrite(FRONT_RIGHT_IN3, HIGH);
  digitalWrite(FRONT_RIGHT_IN4, LOW);
  analogWrite(FRONT_RIGHT_EN, speed);

  // 后左电机
  digitalWrite(REAR_LEFT_IN1, LOW);
  digitalWrite(REAR_LEFT_IN2, HIGH);
  analogWrite(REAR_LEFT_EN, speed);

  // 后右电机
  digitalWrite(REAR_RIGHT_IN3, HIGH);
  digitalWrite(REAR_RIGHT_IN4, LOW);
  analogWrite(REAR_RIGHT_EN, speed);
}
void LEFT()
{
  // 前左电机
  digitalWrite(FRONT_LEFT_IN1, LOW);
  digitalWrite(FRONT_LEFT_IN2, HIGH);
  analogWrite(FRONT_LEFT_EN, speed);

  // 前右电机
  digitalWrite(FRONT_RIGHT_IN3, HIGH);
  digitalWrite(FRONT_RIGHT_IN4, LOW);
  analogWrite(FRONT_RIGHT_EN, speed);

  // 后左电机
  digitalWrite(REAR_LEFT_IN1, HIGH);
  digitalWrite(REAR_LEFT_IN2, LOW);
  analogWrite(REAR_LEFT_EN, speed);

  // 后右电机
  digitalWrite(REAR_RIGHT_IN3, LOW);
  digitalWrite(REAR_RIGHT_IN4, HIGH);
  analogWrite(REAR_RIGHT_EN, speed);
}
void RIGHT()
{
  // 前左电机
  digitalWrite(FRONT_LEFT_IN1, HIGH);
  digitalWrite(FRONT_LEFT_IN2, LOW);
  analogWrite(FRONT_LEFT_EN, speed);

  // 前右电机
  digitalWrite(FRONT_RIGHT_IN3, LOW);
  digitalWrite(FRONT_RIGHT_IN4, HIGH);
  analogWrite(FRONT_RIGHT_EN, speed);

  // 后左电机
  digitalWrite(REAR_LEFT_IN1, LOW);
  digitalWrite(REAR_LEFT_IN2, HIGH);
  analogWrite(REAR_LEFT_EN, speed);

  // 后右电机
  digitalWrite(REAR_RIGHT_IN3, HIGH);
  digitalWrite(REAR_RIGHT_IN4, LOW);
  analogWrite(REAR_RIGHT_EN, speed);
}
void stopMotors()
{
  // 前左电机
  analogWrite(FRONT_LEFT_EN, 0);

  // 前右电机
  analogWrite(FRONT_RIGHT_EN, 0);

  // 后左电机
  analogWrite(REAR_LEFT_EN, 0);

  // 后右电机
  analogWrite(REAR_RIGHT_EN, 0);
}
void LEFTFRONT()
{
  // 前左电机
  digitalWrite(FRONT_LEFT_IN1, LOW);
  digitalWrite(FRONT_LEFT_IN2, LOW);
  analogWrite(FRONT_LEFT_EN, 0);

  // 前右电机
  digitalWrite(FRONT_RIGHT_IN3, HIGH);
  digitalWrite(FRONT_RIGHT_IN4, LOW);
  analogWrite(FRONT_RIGHT_EN, speed);

  // 后左电机
  digitalWrite(REAR_LEFT_IN1, HIGH);
  digitalWrite(REAR_LEFT_IN2, LOW);
  analogWrite(REAR_LEFT_EN, speed);

  // 后右电机
  digitalWrite(REAR_RIGHT_IN3, LOW);
  digitalWrite(REAR_RIGHT_IN4, LOW);
  analogWrite(REAR_RIGHT_EN, 0);
}
void RIGHTBEHIND()
{
  // 前左电机
  digitalWrite(FRONT_LEFT_IN1, LOW);
  digitalWrite(FRONT_LEFT_IN2, LOW);
  analogWrite(FRONT_LEFT_EN, speed);

  // 前右电机
  digitalWrite(FRONT_RIGHT_IN3, LOW);
  digitalWrite(FRONT_RIGHT_IN4, HIGH);
  analogWrite(FRONT_RIGHT_EN, speed);

  // 后左电机
  digitalWrite(REAR_LEFT_IN1, LOW);
  digitalWrite(REAR_LEFT_IN2, HIGH);
  analogWrite(REAR_LEFT_EN, speed);

  // 后右电机
  digitalWrite(REAR_RIGHT_IN3, LOW);
  digitalWrite(REAR_RIGHT_IN4, LOW);
  analogWrite(REAR_RIGHT_EN, 0);
}
void RIGHTFRONT()
{
  // 前左电机
  digitalWrite(FRONT_LEFT_IN1, HIGH);
  digitalWrite(FRONT_LEFT_IN2, LOW);
  analogWrite(FRONT_LEFT_EN, speed);

  // 前右电机
  digitalWrite(FRONT_RIGHT_IN3, LOW);
  digitalWrite(FRONT_RIGHT_IN4, LOW);
  analogWrite(FRONT_RIGHT_EN, 0);

  // 后左电机
  digitalWrite(REAR_LEFT_IN1, LOW);
  digitalWrite(REAR_LEFT_IN2, LOW);
  analogWrite(REAR_LEFT_EN, 0);

  // 后右电机
  digitalWrite(REAR_RIGHT_IN3, HIGH);
  digitalWrite(REAR_RIGHT_IN4, LOW);
  analogWrite(REAR_RIGHT_EN, speed);
}
void LEFTBEHIND()
{
  // 前左电机
  digitalWrite(FRONT_LEFT_IN1, LOW);
  digitalWrite(FRONT_LEFT_IN2, HIGH);
  analogWrite(FRONT_LEFT_EN, speed);

  // 前右电机
  digitalWrite(FRONT_RIGHT_IN3, LOW);
  digitalWrite(FRONT_RIGHT_IN4, LOW);
  analogWrite(FRONT_RIGHT_EN, 0);

  // 后左电机
  digitalWrite(REAR_LEFT_IN1, LOW);
  digitalWrite(REAR_LEFT_IN2, LOW);
  analogWrite(REAR_LEFT_EN, 0);

  // 后右电机
  digitalWrite(REAR_RIGHT_IN3, LOW);
  digitalWrite(REAR_RIGHT_IN4, HIGH);
  analogWrite(REAR_RIGHT_EN, speed);
}
//以下的全是方向函数

void setup() 
{
  // 设置所有引脚为输出模式
  pinMode(FRONT_LEFT_IN1, OUTPUT);
  pinMode(FRONT_LEFT_IN2, OUTPUT);
  pinMode(FRONT_LEFT_EN, OUTPUT);
  pinMode(FRONT_RIGHT_IN3, OUTPUT);
  pinMode(FRONT_RIGHT_IN4, OUTPUT);
  pinMode(FRONT_RIGHT_EN, OUTPUT);
  pinMode(REAR_LEFT_IN1, OUTPUT);
  pinMode(REAR_LEFT_IN2, OUTPUT);
  pinMode(REAR_LEFT_EN, OUTPUT);
  pinMode(REAR_RIGHT_IN3, OUTPUT);
  pinMode(REAR_RIGHT_IN4, OUTPUT);
  pinMode(REAR_RIGHT_EN, OUTPUT);

  // 初始化Blinker
  Blinker.begin();  
  JOY1.attach(joystick1_callback);
  Button1.attach(Button1_callback);
  Button2.attach(Button2_callback);
  Button3.attach(Button3_callback);
  Button4.attach(Button4_callback);
  Button5.attach(Button5_callback);
  Button6.attach(Button6_callback);
}

void loop() 
{
  // 处理Blinker事件
  Blinker.run();
}